#include "fit.h"

double FunPlane(const cv::Mat& input, const cv::Mat& lms);

Fit::Fit() : lm_(new LM())
{
}


Fit::~Fit()
{
    if (lm_ != NULL)
    {
        delete lm_; 
        lm_ = NULL; 
    }
}


void Fit::SetModel(const std::vector<float>& model)
{
	model_ = model; 
}


void Fit::GetModel(std::vector<float>& model)
{
	model = model_; 
}


float Fit::GetRealPrecision() const
{
	return lm_->GetRealPrecision(); 
}


int Fit::GetRealIter() const
{
	return lm_->GetRealIter(); 
}


void Fit::SetMaxIter(int maxIter)
{
    lm_->SetMaxIter(maxIter); 
}


void Fit::SetDeriveStep(float step)
{
    lm_->SetDeriveStep(step); 
}


void Fit::SetPrecision(float prec)
{
    lm_->SetPrecision(prec); 
}


bool Fit::Compute()
{
    int N = static_cast<int>(model_.size());
    cv::Mat params = cv::Mat(N, 1, (sizeof(float) / 4 + 4));
    for (int i = 0; i < N; i++)
    {
        params.at<float>(i, 0) = model_[i];
    }
    lm_->InitParam(params);
    if (!lm_->Compute(&FunPlane, input_, output_))
    {
        return false;
    }
    cv::Mat params_t = lm_->GetParams();
    for (int i = 0; i < params_t.rows; i++)
    {
        model_[i] = (static_cast<float>(params_t.at<float>(i, 0)));
    }
    return true;
}


double FunPlane(const cv::Mat& input, const cv::Mat& lms)
{
    float A = lms.at<float>(0, 0);
    float B = lms.at<float>(1, 0);
    float C = lms.at<float>(2, 0);
    float D = lms.at<float>(3, 0);
    float x = input.at<float>(0, 0);
    float y = input.at<float>(0, 1);
    float z = input.at<float>(0, 2);
    return static_cast<double>(A * x + B * y + C * z + D);
}